#pragma config(Sensor, in1,    pot_sensor,     sensorPotentiometer)
#pragma config(Sensor, dgtl1,  front_right_limit_switch, sensorTouch)
#pragma config(Sensor, dgtl2,  back_right_limit_switch, sensorTouch)
#pragma config(Motor,  port1,           left_motor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           claw_motor,    tmotorVex393, openLoop)
#pragma config(Motor,  port7,           joint_motor,   tmotorVex393, openLoop)
#pragma config(Motor,  port10,          right_motor,   tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void forward_distance( float inches );

task main()
{
	while(true)
	{
		//2787 is the height of the arm to get the ball off the pipe

		if(SensorValue[front_right_limit_switch] ==1 && SensorValue[back_right_limit_switch]== 1)
		{
			motor[left_motor]=63;
			motor[right_motor]=63;
		}
		if(SensorValue[front_right_limit_switch]==0)
		{
			motor[left_motor]=31;
			motor[right_motor]=-31;
			wait1Msec(250);
			motor[left_motor]=0;
			motor[right_motor]=0;
			wait1Msec(1000);
			forward_distance(10);
		}
		if(SensorValue[back_right_limit_switch]==0)
		{
			motor[right_motor]=31;
			motor[left_motor]=-31;
			wait1Msec(250);
			motor[left_motor]=0;
			motor[right_motor]=0;
			wait1Msec(1000);
			forward_distance(10);
		}
	}
}
void forward_distance( float inches )
{
	int	milliseconds=(inches-5.96)/.0255;
	motor[left_motor]=127;
	motor[right_motor]=127;
	wait1Msec(milliseconds);
	motor[left_motor]=0;
	motor[right_motor]=0;
}
